% !Mode:: "TeX:UTF-8" 

\BiChapter{基于相对测量的包围构型协同控制方法}{Surrounding formation control using relative measurements}

\BiSection{引言}{Introduction}

本章研究的包围构型是指一组跟随者智能体在分布式自组织控制器作用下，可以将另一组静止的领导者智能体，最终包围在以跟随者位置为顶点所组成的凸包构型中。包围构型在合作和非合作的集群任务中都具有重要研究意义。针对包围构型的编队控制方法最早于文献\cite{chen2010surrounding}中被提出，Fei Chen等人对一般的包围控制和平衡包围控制问题分别进行了研究，提出了一类分布式控制律，并借助李雅普诺夫稳定理论验证了控制方法的稳定性。此后，包围控制被应用于多种不同的任务场景，如文献\cite{sharghi2018adaptive}研究了智能体动力学模型存在扰动的自适应包围控制问题；文献\cite{zhang2019finite}研究了针对多个目标的有限时间包围控制问题；以及文献\cite{li2018cooperative}研究了具有拉格朗日动力学的多智能体网络防碰撞包围控制问题。


然而当前的包围构型协同控制方法存在如下两个问题：

(1) 除文献\cite{li2018cooperative}研究了具有特殊动力学的多智能体系统外，其他已有研究成果仅考虑具有一阶质点模型的多智能体系统，控制器的期望指令是节点的速度。然而大多数实际的运动平台都需要分析平台的受力情况，因此设计加速度控制器具有更为显著的实际意义。

(2) 现有的研究方法均假设节点在绝对坐标系下的位姿状态是已知的，且可以在多智能体系统中进行交互。然而在GNSS受限拒止环境中，直接观测节点在绝对坐标系下的位姿状态是十分困难的。因此传统使用位置坐标设计的协同控制方法不再适用于GNSS受限拒止条件下集群的构型变换。

针对上述问题，本章旨在利用每个节点在自身局部坐标系下对邻接节点的相对测量数据，研究具有二阶运动学的多智能体系统包围构型协同控制方法。本章给出包围构型控制问题的数学定义，并以一维空间中的包围构型控制问题为例设计控制方法，随后合理地拓展至高维空间。本章首先研究了在GNSS信号未被拒止条件下，基于绝对定位信息的包围构型协同控制方法，提出了两个基于速度和位置状态感知的分布式控制律，分别对应于领导者集群平均位置已知和平均位置未知的两种情况。并且基于图论和李雅普诺夫稳定理论，验证了控制律的有效性。在此基础上，本章进一步设计了在GNSS信号受限拒止情况下，基于相对测量信息的包围构型分布式协同控制律。设计了基于相对测量的自组织估计算法，使得每个跟随者节点仅依赖于其对邻接节点的测量，估计该节点与所有领导者节点之间的相对距离，从而计算当前跟随者节点距领导者集群平均位置的相对距离。最后，通过二维空间的包围控制仿真实例验证了本章所提方法的正确性和可行性。

\BiSection{问题描述}{Problem formulations}	

\BiSubsection{通用符号}{General notations}

本章后续内容中将使用$\mathbb{R}^{d}$表示$d$维欧几里得空间。当上下文没有明确定义矩阵维度时，使用$\vect{I}$表示合适维度的单位矩阵。$\vect{0}$和$\vect{1}$表示具有合适维度的全1和全0向量。给定一个集合$\vect{S}$，使用$|\vect{S}|$表示该集合中包含的元素个数。定义函数$sgn(\cdot)$为符号函数，当输入为标量时，返回输入数据的正负符号；当输入为向量时，例如输入为$\vect{x} = [x_1,x_2,\cdots,x_d]^T$,则该符号函数的输出为$sgn(x)=[sgn(x_1),sgn(x_2),\cdots,sgn(x_d)]$. 给定一个向量$\vect{x} \in \mathbb{R}^d$ 以及一个集合$\vect{S} \subset \mathbb{R}^d$, 定义如下操作
$$||\vect{x} - \vect{S}|| \triangleq \inf\limits_{\vect{y} \in \vect{S}} ||\vect{x} - \vect{y}||$$
上式中$||\cdot||$表示取输入的欧几里得范数。

\BiSubsection{图论}{The graph theory}

图论是对多智能体系统进行建模和分析的重要工具。下面给出本节将使用到的和图论有关的符号定义：

由$n$个节点或智能体组成的内部存在数据交互和协同的多体系统，可以被建模成一个具有$n$个节点的图$\mathcal{G}=\{\mathcal{V},\mathcal{E},\vect{A}\}$. 其中$\mathcal{V}=\left\{1,2,\cdots,n\right\}$表示系统中节点的集合。$\mathcal{E}$为节点之间的边的集合，一条边表示两个节点之间存在某种实际互动和联系。例如，这种联系可以用于表示两个节点之间存在相互通信，或者是相对测量等系统内部的协同行为。为了区分节点之间交的强度或者重要性，对每个边引入正实数$a_{ij}\ge0,\forall i,j \in \mathcal{V}$表示两个节点间交互的权值。如果节点$i$和节点$j$之间不存在交互，则$a_{ij}=0$,因此$\mathcal{E}:=\{(i,j) \in \mathcal{V} \times \mathcal{V} | \forall a_{ij}\ne 0 \}$。矩阵$\vect{A}$存储了系统内部所有节点之间边的权值信息。当$a_{ij} = a_{ji}, \forall i,j \in \mathcal{V}$,则图$\mathcal{G}$为无向图，否则称之为有向图。

如果从任意的节点$i$出发，都能找到一组节点序列，使得序列中任意位置的前后两个节点都是邻接节点，从而最终能够到达$j$节点，则称节点$j$是连通的。如果$\mathcal{V}$中的所有节点都是连通的，则称这个图是连通图。对于节点$i$,定义$\vect{N}_i$为它的邻接节点集合，即$\vect{N}_i = \left\{ j \in \mathcal{V}:(i,j) \in \mathcal{E},j \ne i\right\}$。定义对角矩阵$\vect{D} = diag\{ [d_{1}, d_{2}, \cdots , d_{n}] \}$为图$\mathcal{G}$的度矩阵，对角线上的元素为$d_{i} = \sum_{j \in \vect{N}_i}a_{ij}$。因此图$\mathcal{G}$的拉普拉斯矩阵为$\vect{L}= \vect{D}-\vect{A}$。对于无向图$\mathcal{G}$，其拉普拉斯矩阵有$\vect{L}=\vect{L}^T \ge 0$，且全一向量$\vect{1}^T = [1,1,\cdots,1]^T$是对应于拉普拉斯矩阵最小特征值$\lambda_1$的特征向量，并且该最小特征值为$0$。当图$\mathcal{G}$为无向连通图时，其拉普拉斯矩阵$\vect{L}$有且仅有一个零特征值，其余特征值都大于零，即$0=\lambda_1(\vect{L}) < \lambda_2(\vect{L})\le\cdots\le\lambda_n(\vect{L})$。另外$\lambda_2(\vect{L})$也被称为图$\mathcal{G}$的几何连通度。

\BiSubsection{包围构型控制问题}{Definition of surrounding control}

\begin{figure}[htbp]
	\centering
	\includegraphics[width = 0.95\textwidth]{figures_SURR/SurroundingControlDeom.eps}
	\bicaption[fig. SurroundingControlDeom]{}{包围控制问题示意图}{Fig.$\!$}{Illustration for the surrounding control problem}\vspace{0em}
\end{figure}


如图~\ref{fig. SurroundingControlDeom}所示，假设有两组多智能体集群，其中一组被称为领导者，有$m$个节点，记$\vect{V}_L=\{ 1,2,\cdots, m \}$为领导者集合；另一组被称为跟随者，共有$n$个节点，记为$\vect{V}_F=\{ 1,2,\cdots, n \}$为跟随者集合。两个集合满足$\vect{V}_F \bigcup \vect{V}_L = \mathcal{V}$。与$\vect{N}_i$的含义类似，定义$\vect{N}_i^F = \{ \forall j \in \vect{V}_L | (i,j) \in \mathcal{E} \}$ 为与第$i$个跟随者存在通信的所有领导者的集合；$\vect{N}_i^L = \{ \forall j \in \vect{V}_F | (i,j) \in \mathcal{E} \}$， 为与第$i$个领导者存在通信的所有跟随者的集合。
假设领导者是保持静止，跟随者可以通过输入控制指令进行运动，跟随者的运动过程为二阶质点模型，即
\begin{equation} \label{eq. SecondOrderDynamics}
	\dot{x}_i = v_i, \dot{v}_i = u_i, \forall i \in \vect{V}_F
\end{equation}

则包围控制问题的目标是通过设计跟随者集群的加速度控制输入$\vect{u}_i,\forall i \in \vect{V}_F$,使得所有领导者最终处于由跟随者节点所组成的凸包构型内部，即形成包围构型。该过程的数学定义如下：
\begin{definition} \label{Def. SurroundingProblem}
	包围构型控制问题是指为跟随者设计合适的控制输入$u_i,\forall i \in \vect{V}_F$，使得跟随者最终包围领导者形成一个凸的环绕构型，即
	\begin{equation}
		\mathop {\lim }\limits_{t \to \infty } \left\| {{x_j}\left( t \right) - co\left( {{\vect{V}_F}} \right)} \right\| = 0, \forall j \in \vect{V}_L
	\end{equation} 
	式中$co(\vect{V}_F)$表示由跟随者组成的凸包构型。
\end{definition}

由图~\ref{fig. SurroundingControlDeom}可以看出，高维度运动空间的包围构型控制问题可以分解为多个一维运动空间上的包围控制问题。当每个维度上的包围构型都实现之后，高维度空间的包围问题随即得到解决。因此，后文将以一维运动空间为例，进行控制器设计和稳定性分析。然后将该方法合理地拓展至高维度的运动空间之中。
在一维空间中，包围控制问题的目标构型是形成所有领导者节点的外围包络。因此一个理想的构型是形成一个集合，该集合以领导者平均位置为中心，覆盖半径$\xi$大于领导者集群中任意节点与平均位置的距离$\operatorname{max}_{i \in \vect{V}_L}||x_{i} - \bar{x}_L||$，如图~\ref{fig. SurroundingControlDeomOneDim}所示。

\begin{figure}[htbp]
	\centering
	\includegraphics[width = 0.95\textwidth]{figures_SURR/SurroundingControlDeomOneDim.eps}
	\bicaption[fig. SurroundingControlDeomOneDim]{}{一维空间案例下的包围控制问题期望构型}{Fig.$\!$}{Expected formation of the surrounding problem for one dimensionality case}\vspace{0em}
\end{figure}

后文将按照智能体是否可以获得其在绝对坐标系下的状态观测分为两节，分别进行控制器设计和分析。当绝对状态观测已知时，包围构型控制器将可以使用位置状态$x_{F_i}, \forall i \in \vect{V}_F$数据作为控制器输入。当无法获得绝对状态观测时，包围控制问题的难点在于跟随者智能体无法确定其相对于领导者集群平均位置之间的相对距离大小。该情况下包围构型的形成将以前一种控制器为基础，通过设计合适的估计方法，仅使用相对观测数据来计算平均距离。

\BiSection{基于绝对位置信息的包围控制方法}{Surrounding control using absolute measurements}
本节先考虑当绝对定位信息可以获取，即GNSS信号仍存在时，如何求解上述的包围控制问题。在设计具体控制方法之前，需要对该问题做出如下合理假设，
\begin{assumption} \label{Ass. surroundingSize}
	\textbf{跟随者集群规模假设}:跟随者的数量满足$n>d+1$，式中$d$为智能体运动空间的维度。
\end{assumption}

\begin{assumption} \label{Ass. NoColinear}
	\textbf{非共线假设}：在跟随者的初始位置中，不存在任意$d$个跟随者节点，使得它们的位置$x_{p_i}, i \in \{ 1,\cdots,d \}, p_i \in \vect{V}_F$满足
	$$ x_i = \sum_{j=1}^{d}a_j x_{p_j},\forall i \in \vect{V}_F$$
	式中$a_j \ge 0, \forall j \in \{ 1,\cdots,d \}$ 并且$a_j \ge 0$ 以及 $\sum_{j=1}^{d}a_j =1$. 	
\end{assumption}

\begin{assumption} \label{Ass. InitalTopology}
	\textbf{初始通信拓扑假设：} 初始情况下，跟随者集群内部的通信拓扑上是全连接的，且权值矩阵是二进制赋值。
\end{assumption}

从工程实践的角度分析，集群内部拓扑的形成需要依赖实际的传感器执行相互测量，以及通信设备执行信息交互。由于实际使用的传感器和通信设备存在测量和通信约束，如无线电设备受功率限制存在最大通信范围、相机存在视场角测量约束等，集群内部拓扑随着节点之间的相互位姿状态的改变而变化。因此对于动态集群，全连接拓扑是很难实现的。然而，假设~\ref{Ass. InitalTopology}对全连接拓扑的要求仅在初始条件下。此时所有的跟随者可以从同一个基地出发，因此可以使用其他辅助通信手段实现它们之间的全连接拓扑需求，例如通过基地内的局域网进行信息交互等。因此假设~\ref{Ass. InitalTopology}在工程上是可以实现的。

\begin{assumption} \label{Ass. SwarmTopology}
	\textbf{集群拓扑假设：}
	
	(1) 跟随者集群内部的通信观测拓扑假设为固定且连通的。
	
	(2) 初始条件下，每一个领导者都至少与一个跟随者通信。
\end{assumption}

本节将设计两种分布式控制律，分别对应于领导者平均位置$\bar{\vect{x}}_L = \frac{1}{m} \sum_{j \in \vect{V}_L} \vect{x}_j$已知,和平均位置未知的两种情景。本节首先研究一维欧几里得空间的包围控制问题，即$d=1$的情况。此时集群节点的位置、速度以及控制输入均为标量，即$x_i,v_i,u_i \in \mathbb{R}^1, \forall i \in \mathcal{V}$。然后将本章设计的控制律扩展到更高维度，即$d>1$的情况。

\BiSubsection{领导者平均位置$\bar{x}_L$已知}{Mean position of all leaders is known}

本小节假设领导者的平均位置$\bar{x}_L$对所有跟随者都是已知的。针对集群的二阶运动学模型\eqref{eq. SecondOrderDynamics},设计如下的分布式控制律：
\begin{equation} \label{eq. ControlForAbsolutePosition-KnownAverage}
	\begin{aligned}
		u_i = & k_1 \sum_{j \in \vect{N}_i \cap \vect{V}_F} a_{ij} (v_i - v_j) - k_2 v_i\\
		& + \left( \bar{x}_L + \xi sgn \left\lbrace \sum_{{j \in \vect{N}_i \cap \vect{V}_F}} a_{ij}(0) [ x_i(0) - x_j(0) ] \right\rbrace  - x_i \right) 
	\end{aligned}
\end{equation}
上式中，$a_{ij}$为第$i$和$j$个节点之间的连通权值，$k_1,k_2$和$\xi$ 为三个正的常量参数。$\vect{x}_0 = [x_1(0),x_2(0),\cdots, x_n(0)]^T$为所有跟随者的初始位置状态向量。

式~\eqref{eq. ControlForAbsolutePosition-KnownAverage}所设计的控制器一共包含三项。第一项$k_1 \sum_{j \in \vect{N}_i \cap \vect{V}_F} a_{ij} (v_i - v_j)$为速度一致项，目的是让所有跟随者的速度状态最终达到一致；第二项是单个节点的速度反馈$-k_2v_i$，可以保证所有节点在稳态构型下的速度是零，即达到期望的构型后就不再施加运动；第三项是实现包围控制问题中驱动跟随者形成一个凸包$co(\vect{V}_F)$的关键输入，该项存在的目的是驱动节点在稳定状态下抵达$\bar{x}_L + \xi sgn \left\lbrace \sum_{{j \in \vect{N}_i \cap \vect{V}_F}} a_{ij}(0) [ x_i(0) - x_j(0) ] \right\rbrace$的位置。式中$\xi$为稳定状态下跟随者所形成的凸包与领导者平均位置之间的期望距离。该参数最终将控制凸包的形状，决定包围的构型。符号函数$sgn(\cdot)$依据所有节点的初始位置自动分布节点的运动方向。该控制器仅使用自身的位置、速度状态以及邻接节点的速度状态，又根据假设~\ref{Ass. InitalTopology}节点内的初始位置$\vect{x}_0$也是已知的，因此改控制器是属于分布式控制律。

首先通过下述引理证明，当选用合适的控制参数$k_1,k_2$时，分布式控制律~\eqref{eq. ControlForAbsolutePosition-KnownAverage}可以控制跟随者集群最终将形成一个稳定的凸包$co(\vect{V}_F)$，即 $ \operatorname{lim}\limits_{t \to \infty }v_i=0 , \operatorname{lim}\limits_{t \to \infty }u_i = 0, i \in \vect{V}_F $.
\begin{lemma} \label{lemma2-1. CovexHullStable}
	如果假设~\ref{Ass. surroundingSize}-\ref{Ass. InitalTopology} 都成立，则对于具有二阶运动学~\eqref{eq. SecondOrderDynamics}的跟随者集群，在控制律~\eqref{eq. ControlForAbsolutePosition-KnownAverage}的作用下，当控制参数满足$\frac{k_2}{k_1}>\lambda_{max}(\vect{L})$时，有：$$ \operatorname{lim}\limits_{t \to \infty }v_i=0 , \operatorname{lim}\limits_{t \to \infty }u_i = 0, i \in \vect{V}_F$$
	其中$\vect{L}$为跟随者集群通信拓扑的拉普拉斯矩阵。
\end{lemma}

\begin{proof}
	记$\vect{x} = [x_1,x_2,\cdots,x_n]^T,\vect{v} = [v_1,v_2,\cdots,v_n]^T，\vect{u} = [u_1,u_2,\cdots,u_n]^T$为跟随者集群位置、速度状态以及控制输入的向量形式。则对于整个跟随者集群，控制输入~\eqref{eq. ControlForAbsolutePosition-KnownAverage}可改写为如下的向量形式：
	$$\vect{u} = k_1\vect{L}\vect{v} - k_2 \vect{v} + \left[ \bar{x}_L \vect{1} + \xi sgn(\vect{L}(0)\vect{x}_0) - \vect{x} \right]$$
	由于符号函数项$sgn(\cdot)$以及领导者平均位置项$\bar{x}_L\vect{1}$在确定后均为常量，因此对上式求导后可得：
	\begin{equation} \label{eq. derivative1}
		\dot{\vect{u}} = k_1 \vect{L} \dot{\vect{v}} - k_2 \dot{\vect{v}} - \dot{\vect{x}}
	\end{equation}
	定义如下函数作为该问题的李雅普诺夫函数：
 	\begin{equation*}
		\begin{aligned}
			V&=\frac{1}{2}(\vect{v}^T \vect{v} + \vect{u}^T \vect{u})\\
			&=(\vect{v}^T \vect{v} + \dot{\vect{u}}^T \dot{\vect{u}})
		\end{aligned}
	\end{equation*}
	则对上式两边求一阶导数可得：
	\begin{equation*} 
		\begin{aligned}
			\dot{V} &= \vect{v}^T\dot{\vect{v}} + \dot{\vect{v}}^T \ddot{\vect{v}}\\
			&= \dot{\vect{v}}^T(\vect{v}+\ddot{\vect{v}})\\
			&= \dot{\vect{v}}^T(\vect{v}+\dot{\vect{u}})
		\end{aligned}
	\end{equation*}
	将式~\eqref{eq. derivative1}带入上式，并考虑运动学$\dot{\vect{x}} = \vect{v}$，可得
 	\begin{equation*}
		\begin{aligned}
			\dot{V} &= \dot{\vect{v}}^T(\vect{v} + k_1\vect{L} \dot{\vect{v}} - k_2 \dot{\vect{v}} - \dot{\vect{x}})\\
			&= \dot{\vect{v}}^T(k_1\vect{L}-k_2\vect{I})\dot{\vect{v}}\\
			&\le [k_1\lambda_{max}(\vect{L})-k_2]||\dot{\vect{v}}||^2
		\end{aligned}
	\end{equation*}
	当$\frac{k_2}{k_1}>\lambda_{max}(\vect{L})$时，有$k_1 \lambda_{max}(\vect{L}) - k_2 < 0$, 则$\dot{V} < 0 $。 因此可以保证$ \operatorname{lim}\limits_{t \to \infty }v_i=0 , \operatorname{lim}\limits_{t \to \infty }u_i = 0, i \in \vect{V}_F$。
	
	引理~\ref{lemma2-1. CovexHullStable}得证。
\end{proof}

当通信观测拓扑的权值采用二进制赋值时，即当两个节点存在连接时$a_{i,j} = 1$，否则$a_{i,j} = 0$，则根据圆盘定理\citeup{bell1965gershgorin}，定理~\ref{lemma2-1. CovexHullStable}中参数$k_1,k_2$的选择条件可以进一步简化为$\frac{k_2}{k_1} > 2(n-1)$。如果权值矩阵不是二进制赋值，则需要获取跟随者集群的通信拓扑权值矩阵，计算拉普拉斯矩阵的最大特征值以确保参数选取的正确性。

引理~\ref{lemma2-1. CovexHullStable}表明，跟随者集群在控制律~\eqref{eq. ControlForAbsolutePosition-KnownAverage}的作用下，最终可以形成一个稳定的凸包构型。为了实现对领导者的包围，该构型需要跟随者分布在领导者的两侧，即需要进一步证明，在稳定状态下，领导者集群的两侧都分布着跟随者。针对跟随者集群，定义两个特殊的集合， $\vect{M}_{in} = \{ i| i \in \vect{V}_F,x_i = \operatorname{min}_{j \in \vect{V}_F} x_j \}$ 以及 $\vect{M}_{ax} = \{ i| i \in V_F,x_i = \operatorname{max}_{j \in \vect{V}_F} x_j\}$。下面的定理将证明，在稳定状态下，所有的跟随者集群都将包括到这两个集合中，并且还将给出每个跟随者运动的方向判断依据。

\begin{lemma} \label{lemma2-2 direction}
	如果假设~\ref{Ass. surroundingSize}-\ref{Ass. InitalTopology} 都成立，则对于具有二阶运动学~\eqref{eq. SecondOrderDynamics}的跟随者集群，在控制律~\eqref{eq. ControlForAbsolutePosition-KnownAverage}的作用下，有
	\begin{equation*} 
		\left\{
		\begin{aligned}
			\mathop {lim}\limits_{t \to \infty } \vect{M}_{in} = \{ i| i \in \vect{V}_F,x_i(0) < \bar{x}_F(0) \} \\
			\mathop {lim}\limits_{t \to \infty } \vect{M}_{ax} = \{ i| i \in \vect{V}_F,x_i(0) \ge \bar{x}_F(0) \}
		\end{aligned}
		\right.
	\end{equation*}
	式中，$\bar{x}_F(0) = \frac{1}{n} \sum_{j \in \vect{V}_F} x_j(0)$为跟随者集群在初始时间的平均位置，且$\mathop {lim}\limits_{t \to \infty } |\vect{M}_{in}| \neq \emptyset$, $\mathop {lim}\limits_{t \to \infty } |\vect{M}_{ax}| \neq \emptyset$.
\end{lemma}

\begin{proof} 
	由引理~\ref{lemma2-1. CovexHullStable}可知，在控制输入~\eqref{eq. ControlForAbsolutePosition-KnownAverage}的作用下，当跟随者集群形成稳定构型时，每个节点的控制输入都为零，因此对任意跟随者$i\in V_F$,有：
	\begin{equation*}
		\mathop {lim}\limits_{t \to \infty }x_i= \bar{x}_L + \xi \operatorname{sgn} \left\lbrace \sum_{j \in N_i^s} a_{ij}(0) [ x_i(0) - x_j(0) ] \right\rbrace
	\end{equation*}
	即
	\begin{equation*}
		\left\{
		\begin{aligned}
			\mathop {lim}\limits_{t \to \infty }\{ \mathop {min} \limits_{j \in V_F} x_j \}  = \bar{x}_L - \xi \\
			\mathop {lim}\limits_{t \to \infty }\{ \mathop {max} \limits_{j \in V_F} x_j \}  = \bar{x}_L + \xi
		\end{aligned}
		\right.
	\end{equation*}
	因此，由上式可知，跟随者集群中节点的运动方向是由符号函数$sgn \left\lbrace \sum_{{j \in \vect{N}_i \cap \vect{V}_F}} a_{ij}(0) [ x_i(0) - x_j(0) ] \right\rbrace$决定的。因此对集群中的任意跟随者$p\in \vect{V}_F$，在稳定构型下有：
	 $$\mathop {lim}\limits_{t \to \infty } x_p = \bar{x}_L + \xi \operatorname{sgn} \left\lbrace \sum_{j \in \vect{N}_i \cap \vect{V}_F} a_{pj}(0) [ x_p(0) - x_j(0) ] \right\rbrace$$ 
	由假设~\ref{Ass. InitalTopology}可知，$a_{pj}(0)=1,\forall j \in \{1,\cdots,p-1,p+1,\cdots,n\}$，因此，
 	$$
	\begin{aligned}
		\sum_{j \in \vect{N}_i \cap \vect{V}_F}  a_{pj}(0) & [ x_p(0) - x_j(0) ] = \sum_{j=1,j \ne p }^{n}[ x_p(0) - x_j(0) ]\\
		& = (n-1) x_p(0) - \sum_{j=1,j \ne p }^{n} x_j(0) \\
		& = n \left\lbrace x_p (0) - \frac{1}{n} \sum_{j=1}^{n} x_j(0) \right\rbrace \\
		& = n [ x_p (0) -\bar{x}_F (0) ]
	\end{aligned}
	$$
	因此，当 $x_p(0) < \bar{x}_F (0)$时,
	$$\mathop {lim}\limits_{t \to \infty }x_p  = \bar{x}_L - \xi \Rightarrow x_p \in \vect{M}_{in} $$
	否则
	$$\mathop {lim}\limits_{t \to \infty }x_p  = \bar{x}_L + \xi \Rightarrow x_p \in \vect{M}_{ax}$$
	又由假设~\ref{Ass. surroundingSize}可知，$n>d+1 >1$, 因此$|\vect{M}_{in}|>0$,$|\vect{M}_{ax}|>0$。至此引理~\ref{lemma2-2 direction}得证。
\end{proof}

\begin{theorem} \label{th2-1. mainTheorem}
	如果假设~\ref{Ass. surroundingSize}-\ref{Ass. InitalTopology} 都成立，则对于具有二阶运动学~\eqref{eq. SecondOrderDynamics}的跟随者集群，在控制律~\eqref{eq. ControlForAbsolutePosition-KnownAverage}的作用下，如果$\frac{k_2}{k_1} > \lambda_{max}(\vect{L})$，并且$\bar{x}_L$已知，$\xi \ge \operatorname{max}_{i \in \vect{V}_L} ||x_i - \bar{x}_L||$，则定义~\ref{Def. SurroundingProblem}的包围控制问题得解。
\end{theorem}

\begin{proof}
	由引理~\ref{lemma2-1. CovexHullStable}和~\ref{lemma2-2 direction}可知在稳定状态下有：
	$$[\bar{x}_L-\xi,\bar{x}_L+\xi] \subseteq [\mathop {\max}\limits_{j \in \vect{V}_F} x_j, \mathop {\min}\limits_{j \in \vect{V}_F} x_j]$$
	因此对于任意一个跟随者$i \in  \vect{V}_L$，当$t \rightarrow \infty$时，有
	$$x_i \in [\bar{x}_L-\xi,\bar{x}_L+\xi] \subseteq [\mathop {\max}\limits_{j \in \vect{V}_F} x_j, \mathop {\min}\limits_{j \in \vect{V}_F} x_j]$$
	因此可知
	$$\mathop {\lim }\limits_{t \to \infty } \left\| {{x_j}\left( t \right) - co\left( {{\vect{V}_F}} \right)} \right\| = 0$$
	
	至此定理~\ref{th2-1. mainTheorem}得证。
\end{proof}


\BiSubsection{领导者平均位置$\bar{x}_L$未知}{Mean position of all leaders is unknown}

前一节假设领导者集群的平均位置对所有跟随者已知。然而在实际任务中，$\bar{x}_L$是未知的。本小节设计一类一致性估计器，基于跟随者对领导者绝对未知的感知信息，通过跟随者在集群内部交换彼此的估计值，协同计算领导者集群的平均位置信息。跟随者$i$上运行的估计器为：
\begin{equation} \label{eq. Estimator}
	\dot{y_i} = k_3 \sum_{j \in \vect{N}_i \cap \vect{V}_F} (y_j - y_i),\forall i \in \vect{V}_F
\end{equation}
式中$y_i$是第$i$个跟随者对领导者集群平均位置$\bar{x}_L$的估计值，$k_3>0$是估计器的常量参数。估计器的初始状态为：
\begin{equation} \label{eq. IntialEstimator}
	y_i(0) = \frac{n}{m} \sum_{j \in \vect{N}_i^L} \left[ \frac{1}{|\vect{N}_j^F|} x_j(0)\right], \forall i \in \vect{V}_F
\end{equation} 
上式中$\vect{N}_i^L$为与第$i$个领导者存在通信观测连通的所有跟随者的集合，$\vect{N}_i^F$为与第$i$个跟随者存在通信观测连通的所有领导者的集合。本质上，式~\eqref{eq. Estimator}的估计器是一类针对一阶多智能体集群的平均一致性控制协议。当通信拓扑图是连通的，且任意边的通信权值为$a_{ij}=1$，则该控制器将驱使所有的一阶状态$y_i$收敛于式~\eqref{eq. IntialEstimator}给出的集群初始状态的平均值。下面的引理将对这一论述给出具体的证明。

\begin{lemma} \label{lemma2-3. EstimatorStability}
	如果假设~\ref{Ass. SwarmTopology}成立，则由式~\eqref{eq. Estimator}-\eqref{eq. IntialEstimator}组成的估计器的状态$y_i$最终将渐进收敛于$\bar{x}_L$，即
	$$\mathop {\lim} \limits_{t \to \infty }||y_i - \bar{x}_L|| = 0, \forall i \in \vect{V}_F$$
\end{lemma}

\begin{proof}
	定义误差$e_i = y_i - \bar{x}_L$，由于领导者集群是假设静止的，$\bar{x}_L$为常量。则对$e_i$求导数，且引入~\eqref{eq. Estimator}可得：
	$$\dot{e}_i = k_3 \sum_{j \in \vect{N}_i \cap \vect{V}_F} (e_j - e_i) = -k_3\vect{L}\vect{e}$$
	选取李雅普诺夫函数为
	$$V_e = \frac{1}{2}\vect{e}^T \vect{e},$$
	其中$\vect{e} = [e_1,e_2,\cdots,e_n]^T$。则对该李雅普诺夫函数两边求导数可得：
	$$
	\begin{aligned}
		\dot{V_e} &= \vect{e}^T \dot{\vect{e}} = k_3\vect{e}^T(-\vect{L}\vect{e}) \\
		&\le -k_3 \lambda_2 (\vect{L}) ||\vect{e}||^2
	\end{aligned}
	$$ 
	由假设~\ref{Ass. SwarmTopology}可知，跟随者集群的通信拓扑是固定且连通的，因此根据图论知识可得，$\lambda_2(\vect{L}) > 0$，因此$\dot{V_e}<0$，即$\mathop{lim} \limits_{t \to \infty } \vect{e} = \vect{0}$。
	
	至此，引理~\ref{lemma2-3. EstimatorStability}得证。
\end{proof}

基于引理~\ref{lemma2-3. EstimatorStability}，针对领导者集群平均位置未知情况下包围控制问题，设计一类分布式控制律如下所示：
\begin{equation} \label{eq. ControllerWithoutXbar}
	\begin{aligned}
		u_i = & k_1 \sum_{j \in \vect{N}_i \cap \vect{V}_F} a_{ij} (v_i - v_j) - k_2 v_i\\
		& + \left( y_i + \xi sgn \left\lbrace \sum_{{j \in \vect{N}_i \cap \vect{V}_F}} a_{ij}(0) [ x_i(0) - x_j(0) ] \right\rbrace  - x_i \right) 
	\end{aligned}
\end{equation}

与控制律~\eqref{eq. ControlForAbsolutePosition-KnownAverage}相比，上式的唯一区别是使用了估计值$y_i$替代了领导者集群平均位置$\bar{x}_L$，下面的引理将证明该控制律的稳定特性：

\begin{lemma} \label{lemma2-4 StabilityObserverController}
	如果假设~\ref{Ass. surroundingSize}-\ref{Ass. SwarmTopology}成立，则对于具备二阶运动学模型~\eqref{eq. SecondOrderDynamics}的跟随者集群，在领导者集群平均位置$\bar{x}_L$未知的情况下，使用控制律~\eqref{eq. ControllerWithoutXbar}，以及估计器~\eqref{eq. Estimator}-\eqref{eq. IntialEstimator}，当控制律和估计器中的参数满足
	$$k_1\lambda_{max}(\vect{L})-k_2+\frac{k_3}{2} < 0$$
	时，跟随者集群最终将形成一个稳定的凸包构型，即实现
	$$\mathop {\lim} \limits_{t \to \infty } v_i = 0, \mathop{\lim} \limits_{t \to \infty } u_i = 0,\forall i \in \vect{V}_F$$
\end{lemma}
\begin{proof}
	定义如下李雅普诺夫函数：
	$$V_1 = \frac{1}{2}\left( \vect{v}^T \vect{v} + \vect{u}^T \vect{u} + a \vect{e}^T \vect{e} \right)$$
	式中$a$为与跟随者集群内部通信拓扑有关的正的常值。定义$\vect{y} = [y_1,y_2,\cdots,y_n]^T$ 为所有跟随者节点上运行的估计器状态的向量，则对上式两侧同时进行求导可得：
	$$\dot{V_1} = \dot{\vect{v}}^T(k_1\vect{L} - k_2 \vect{I}) \vect{\dot{v}} - k_3 \dot{\vect{v}}^T \vect{L} \vect{y} + a \vect{e}^T \vect{\dot{e}} $$
	根据假设~\ref{Ass. SwarmTopology}，有$\vect{L} \vect{1} = \vect{0}$。因此可推出$\vect{L}\vect{y} = \vect{L}(\vect{e} + \bar{x}_L \vect{1}) = \vect{L} \vect{e}$，即
	$$-k_3 \vect{\dot{v}}^T(\vect{L} \vect{y}) = -k_3 \dot{\vect{v}}^T (\vect{L}\vect{e}).$$
	根据Cauchy-Schwartz不等式\citeup{steele2004cauchy}可得：
	$$
	\begin{aligned}
		-\dot{\vect{v}}^T (\vect{L} \vect{e}) &\le ||\dot{\vect{v}}|| \: ||\vect{L}\vect{e}|| \\
		& \le \frac{1}{2} ( \dot{\vect{v}}^T \dot{\vect{v}} +(\vect{L}\vect{e})^T (\vect{L}\vect{e}) ) \\
		& = \frac{1}{2} ( \dot{\vect{v}}^T \dot{\vect{v}} + \vect{e}^T \vect{L}^2 \vect{e} ) \\
		& \le \frac{1}{2} (\dot{\vect{v}}^T \dot{\vect{v}} + \lambda_{max} (\vect{L}^2) \vect{e}^T \vect{e} ) \\
		& \le \frac{1}{2} \dot{\vect{v}}^T \dot{\vect{v}} + 2 (\mathop {\max} \limits_{j \in \vect{V}_F} d_j)^2 \vect{e}^T \vect{e} 
	\end{aligned}
	$$
	上式中$d_j$ 是第$j$个跟随者节点的连通度。李亚普诺夫函数$V_1$的导数可进一步推导为：
	$$
	\begin{aligned}
		\dot{V_1} & \le \dot{\vect{v}}^T (k_1 L - k_2 \vect{I}) \dot{\vect{v}} + \frac{1}{2} k_3 \dot{\vect{v}}^T \dot{\vect{v}} \\
		& + 2 k_3 (\mathop {\max} \limits_{j \in \vect{V}_F} d_j)^2 \vect{e}^T \vect{e} - a k_3 \vect{e}^T \vect{L}\vect{e} \\
		&\le \left( k_1 \lambda_{max} (\vect{L}) - k_2 + \frac{k_3}{2}\right)  ||\dot{\vect{v}}||^2 \\
		& + k_3 \left( 2(\mathop {\max} \limits_{j \in \vect{V}_F} d_j)^2 - a \lambda_2 (\vect{L})\right) ||e||^2
	\end{aligned}
	$$
	针对假设~\ref{Ass. SwarmTopology}中的固定拓扑，一定存在一个常值
	$$a > \frac{2(\mathop {\max} \limits_{j \in \vect{V}_F} d_j)^2}{\lambda_2 (\vect{L})}$$ 
	因此当$k_1 \lambda_{max} (\vect{L}) - k_2 + \frac{k_3}{2}>0$时，满足$\dot{V_1} < 0$。即可以实现：
	$$\mathop {\lim} \limits_{t \to \infty } v_i = 0, \mathop{\lim} \limits_{t \to \infty } u_i = 0,\forall i \in \vect{V}_F$$
	
	至此，引理~\ref{lemma2-4 StabilityObserverController}得证。
\end{proof}


基于引理~\ref{lemma2-3. EstimatorStability}-\ref{lemma2-4 StabilityObserverController}，下面的定理给出了本小节主要成果：

\begin{theorem}\label{th2-2 mainTheoremWithoutX_bar}
	如果假设~\ref{Ass. surroundingSize}-\ref{Ass. SwarmTopology}成立，则对于使用控制律~\eqref{eq. ControllerWithoutXbar}以及估计器~\eqref{eq. Estimator}-\eqref{eq. IntialEstimator}具备二阶运动学~\eqref{eq. SecondOrderDynamics}的跟随者集群，当控制器和估计器的参数满足
	$k_1\lambda_{max}(\vect{L})-k_2+\frac{k_3}{2} < 0$
	且$\xi \ge \mathop{max}_{i \in \vect{V}_L} ||x_i - \bar{x}_L||$
	则定义~\ref{Def. SurroundingProblem}的包围控制问题在领导者集群平均位置$\bar{x}_L$未知的情况下得解。
\end{theorem}

在引理~\ref{lemma2-3. EstimatorStability}-\ref{lemma2-4 StabilityObserverController}的支撑下，该定理的证明与定理~\ref{th2-1. mainTheorem}的证明过程类似，因此此处省略其证明过程。


下面的定理将式~\eqref{eq. ControlForAbsolutePosition-KnownAverage}以及~\eqref{eq. ControllerWithoutXbar}的包围构型控制器拓展到更高维度空间$d>1$。

\begin{theorem} \label{th2-3 higherDimensions}
	当包围构型的任务发生在高纬度空间$d>1$，在假设~\ref{Ass. surroundingSize}-\ref{Ass. SwarmTopology}成立的基础上，对于具备二阶运动学~\eqref{eq. SecondOrderDynamics}的跟随者集群，当它们的初始构型为超矩形时（如二维空间中的矩形，三维空间中的立方体等），如果跟随者在所有维度上均使用控制律~\eqref{eq. ControlForAbsolutePosition-KnownAverage}或带有估计器的控制律~\eqref{eq. ControllerWithoutXbar} \eqref{eq. Estimator} \eqref{eq. IntialEstimator}，且控制器参数满足定理~\ref{th2-1. mainTheorem}和~\ref{th2-2 mainTheoremWithoutX_bar}中的条件时，定义~\ref{Def. SurroundingProblem}中的包围控制控制问题在高纬度空间中得解。
\end{theorem}

\begin{figure}[htbp]
	\centering		
	\subfigure{\label{subfig. discretizationArea}}\addtocounter{subfigure}{-2}
	\subfigure[Estimates for the mean position of leaders]{\subfigure[跟随者对领导者集群平均位置的估计]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Estimate_of_geometric_center1.eps}}}	
	\null\hfill
	\subfigure{\label{subfig. discretizationBoundary}}\addtocounter{subfigure}{-2}
	\subfigure[The velocities of followers]{\subfigure[包围队形形成过程中跟随者实时速度]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Velocities1.eps}}}	
	\bicaption[fig. EstimatedXbarandVelocity]{}{领导者集群平均位置未知情况下分布式协同包围控制律效果}{Fig.$\!$}{Internal variables for the surrouding control law when the mean position of leaders is unknown}
	\vspace{-0em}
\end{figure}

\begin{figure}[htbp]
	\centering		
	\includegraphics[width = 0.7\textwidth]{figures_SURR/Trajectories_real.eps}		
	\bicaption[fig. trajecotoryAbsolutePositions]{}{跟随者集群在包围队形形成过程中的运动轨迹}{Fig.$\!$}{Trajectories of all followers while forming the surround configuration}
	\vspace{-0em}
\end{figure}

\begin{figure}[ht]
	\centering		
	\subfigure{\label{subfig. discretizationArea}}\addtocounter{subfigure}{-2}
	\subfigure[Simulation time t=0s]{\subfigure[仿真时间t=0s]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Moving1s.eps}}}	
	\null\hfill
	\subfigure{\label{subfig. discretizationBoundary}}\addtocounter{subfigure}{-2}
	\subfigure[Simulation time t=1s]{\subfigure[仿真时间t=1s]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Moving101s.eps}}}	
	\null \vfill	
	\subfigure{\label{subfig. discretizationArea}}\addtocounter{subfigure}{-2}
	\subfigure[Simulation time t=5s]{\subfigure[仿真时间t=5s]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Moving501s.eps}}}	
	\null\hfill
	\subfigure{\label{subfig. discretizationBoundary}}\addtocounter{subfigure}{-2}
	\subfigure[Simulation time t=50s]{\subfigure[仿真时间t=50s]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Moving5001s.eps}}}	
	
	\bicaption[fig. SurroundingFormationEvolution]{}{领导者集群平均位置未知情况下分布式协同包围控制律效果}{Fig.$\!$}{The performance of the proposed control law when $\vect{\bar{x}}_L$ is unknown }
	\vspace{-0.5em}
\end{figure}


\BiSubsection{数值仿真结果}{Simulation results} \label{subsec. 2-1 simulation}

本小节验证前文设计的包围控制方法的正确性和有效性。此处仅仿真领导者平均未知$\bar{x}_L$未知的情况，即采用控制器~\eqref{eq. ControllerWithoutXbar}以及估计器~\eqref{eq. Estimator}-\eqref{eq. IntialEstimator}的情况。

本节设置的仿真场景为在二维欧几里得空间（$d=2$）中包括有四个静止的领导者集群以及四个具备二阶运动学模型的跟随者集群。领导者和跟随者的初始空间分布以及它们之间的通信观测连通拓扑关系如图~\ref{fig. SurroundingFormationEvolution} a) 所示。图中蓝色实线表示跟随者集群内部的通信观测拓扑，红色虚线表示跟随者与领导者之间的通信观测拓扑，各类通信观测拓扑权值均设置为$1$。跟随者集群的节点在$x$维度和$y$维度上存在的初始速度均设定为$\vect{v}_0 = [-4, 3, -6, 10]^T m/s$。控制器~\eqref{eq. ControllerWithoutXbar}以及估计器~\eqref{eq. Estimator}-\eqref{eq. IntialEstimator}中的参数选定为：$k_1 = 1,k_2 = 10, k_3 = 2$以及$\xi = 10 > \operatorname{max}_{\forall i \in \vect{V}_L} ||x_i - \bar{x}_L||$。领导者集群在二维空间上的平均位置为$[2,-2]$，图~\ref{fig. EstimatedXbarandVelocity} a) 为四个跟随者节点运行估计器~\eqref{eq. Estimator}-\eqref{eq. IntialEstimator}，通过内部数据交换，对领导者集群平均位置的估计，图中的结果验证了估计器设计的正确性。

图~\ref{fig. SurroundingFormationEvolution} 展示了四个跟随者在给定控制器作用下对领导者集群进行包围构型形成的控制演化过程，结果显示跟随者集群在50s的仿真中成功形成了对领导者集群在二维空间的包围构型，验证了所提方法的正确性和有效性。

\BiSection{基于相对测量信息的包围控制方法}{Surrounding control using relative measurements}

式~\eqref{eq. ControlForAbsolutePosition-KnownAverage}和~\eqref{eq. ControllerWithoutXbar}所设计的控制器都依赖于智能体对自身位置状态$x_i$的感知。不过在GNSS信号受限或拒止环境中，该信息的获取存在极大的困难。本节研究当领导者集群和跟随者集群都无法获取位置状态信息时，如何使用相对距离测量进行包围构型协同控制的问题。本节假设在GNSS受限拒止环境下，跟随者集群节点上装备有相对距离测量传感器，如UWB、激光雷达等，可以执行跟随者集群内部邻接节点之间的相对距离测量，以及执行跟随者和邻接领导者节点之间的相对距离测量。跟随者节点上仍然装备有内部传感器，如IMU或里程计等，可以获得对自身速度状态的测量。

下文将使用$\vect{d}_{ij} = \vect{x}_j - \vect{x}_i, \forall i,j \in \vect{V}_F$表示跟随者集群邻接节点之间的相对距离测量；使用$\vect{d}_{iL_j} = \vect{x}_{L_j} - \vect{x}_i, \forall i \in \vect{V}_F, \forall j \in \vect{V}_L$表示跟随者集群中的节点$i$与领导者集群中的$L_j$节点之间的相对距离测量；使用$\vect{d}_{i\bar{L}} = \bar{\vect{x}}_L - \vect{x}_i$ 表示跟随者集群中的节点$i$与领导者集群平均位置之间的相对距离。由于领导者平均位置处不一定存在可被跟随者观测的节点，因此$\vect{d}_{i\bar{L}}$的值一般是不存在的。

本节依然首先从$d=1$的一维欧几里得空间开始分析，基于控制器~\eqref{eq. ControlForAbsolutePosition-KnownAverage}和~\eqref{eq. ControllerWithoutXbar}的设计基础，分别针对${d}_{i\bar{L}}$已知和${d}_{i\bar{L}}$未知的两种情况，设计对应的分布式协同控制方法。此时，所有的相对观测均为标量。

\BiSubsection{平均距离已知}{Mean distance between leaders and followers is known}

本小节首先假设任意跟随者节点$i$相对于领导者集群的平均位置的距离${d}_{i\bar{L}}$是已知的，此时根据式~\eqref{eq. ControlForAbsolutePosition-KnownAverage}以及相对距离的定义，设计如下基于相对测量信息的分布式控制律：
\begin{equation} \label{eq. Controller_d_iL_Known}
	\begin{aligned}
		u_i = & k_1 \sum_{j \in \vect{N}_i \cap \vect{V}_F} a_{ij} (v_i - v_j) - k_2 v_i\\
		& + \left[ {d}_{i\bar{L}} - \xi sgn \left( \sum_{{j \in \vect{N}_i \cap \vect{V}_F}} a_{ij}(0) d_{ij}(0) \right) \right]
	\end{aligned}
\end{equation}
上式中$d_{ij}(0)$表示领导者集群内部节点之间在初始时刻的相对距离测量。

下面的定理给出了控制律~\ref{eq. Controller_d_iL_Known}对包围构型协同控制问题的控制效果。

\begin{theorem} \label{th2-4. d_i_Lknown}
	由于GNSS受限或拒止而导致领导者集群和跟随者集群的位置状态无法获得的情况下，如果假设~\ref{Ass. surroundingSize}-\ref{Ass. InitalTopology} 都成立，则对于具有二阶运动学~\eqref{eq. SecondOrderDynamics}的跟随者集群，采用控制律~\eqref{eq. Controller_d_iL_Known}，控制参数满足$\frac{k_2}{k_1} > \lambda_{max}(\vect{L})$，并且${d}_{i\bar{L}}$已知，$\xi \ge \operatorname{max}_{i \in \vect{V}_L} ||x_i - \bar{x}_L||$，则定义~\ref{Def. SurroundingProblem}的包围控制问题得解。
\end{theorem}

\begin{proof}
	当${d}_{i\bar{L}}$已知时，根据相对距离$d_{ij},d_{iL_j}$的定义，控制器~\eqref{eq. Controller_d_iL_Known}与控制器~\eqref{eq. ControlForAbsolutePosition-KnownAverage}是等价的。因此定理~\ref{th2-4. d_i_Lknown}的证明与定理~\ref{th2-1. mainTheorem}类似。
\end{proof}

\BiSubsection{平均距离未知}{Mean distance between leaders and followers is unknown}

本小节考虑距离信息$d_{i\bar{L}}$对第$i$跟随者未知的情况。由于在一维欧几里得空间中，所有的测量都是沿着同一个维度进行的，因此对于一个跟随者节点获取的任意两次距离测量$d_{ij_1},d_{ij_2},\forall i \in \vect{V}_F, \forall j_1,j_2 \in \mathcal{V}$ 有如下关系式：
\begin{equation} \label{eq. distanceTwoPoint}
	d_{j_1j_2} =  d_{ij_2} - d_{ij_1}
\end{equation}
又由于领导者节点是静止的，因此距离${d}_{i\bar{L}}$可表达为
\begin{equation} \label{eq. distanceMeanTarget}
	{d}_{i\bar{L}} = \frac{1}{m} \sum_{{j =1}}^{m} d_{iL_{j}}
\end{equation}
分析上式可知，对于跟随者集群中的节点$i$，当其无法与所有领导者进行通信时，$d_{iL_{j}},\forall L_{j} \in \vect{V}_L$是无法全部获取的。因此该节点无法直接计算其相距领导者集群平均位置的距离${d}_{i\bar{L}}$。由此，本小节的主要任务是设计基于局部测量信息的自组织估计方法，使得每一个跟随者节点$i$都可以根据自身获得的测量$d_{ij},\forall j \in \mathcal{V}$，以及从其他邻接节点获得的通信数据$d_{j_1j_2},\forall j_1,j_2 \in \mathcal{V}$通过式~\eqref{eq. distanceTwoPoint}自行估计当前跟随者节点$i$与其他未存在直接测量的领导者节点之间的距离$d_{iL_j}$，并最终通过式~\eqref{eq. distanceMeanTarget}计算$d_{i\bar{L}}$的值。

由于本节处理的问题更为复杂，因此需要更为严格的集群内部观测拓扑。下面将假设~\ref{Ass. SwarmTopology}做如下修改：
\begin{assumption} \label{Ass. SwarmStrenthenTopology}
	\textbf{增强的集群拓扑假设：}
	
	(1) 跟随者集群内部的通信观测拓扑假设为固定且连通的。
	
	(2) 每一个领导者都至少与一个跟随者之间存在观测连通。
\end{assumption}

\begin{algorithm}[tbp]
	\SetAlgoLined
	\KwResult{ 对$d_{i\bar{L}}$的估计：$\hat{d}_{i\bar{L}}$ \; }
	\KwData{ 第$i$个节点对邻接节点的观测数据：$d_{ij}(0),\forall j \in \vect{N}_i, j \neq i$ \;}
	
	首先，节点根据当前时刻的局部测量信息初始化一个局部的增广观测矩阵$\vect{M}_0^i$，作为对全局增广观测矩阵$\vect{M}$的估计。
	
	然后，节点根据式~\eqref{eq. distanceTwoPoint}的关系，对$\vect{M}_0^i$中存在两个以上观测数据的行进行自推演更新，获得$\vect{M}_{0|infer}^i$。
	
	将自推演后的估计矩阵$\vect{M}_{0|infer}^i$通过通信过程共享给其邻接的跟随者节点，并接收其邻接节点传输的估计矩阵$\vect{M}_{0|infer}^j,j\neq i$。
	
	在节点$i$中，将对局部增广测量矩阵的估计值$\vect{M}_{0|infer}^i$与其邻接节点分享的数据$\vect{M}_{0|infer}^j,j\neq i$融合，得到新的局部增广测量矩阵的估计值$\vect{M}_1^i$。该融合方法为$\vect{M}_1^i = \vect{M}_{0|infer}^i \oplus \vect{M}_{0|infer}^j$。
	
	重复1）-4）的更新过程$r$次，直至局部增广测量矩阵的估计值$\vect{M}_{(r)}^i$矩阵中对应于$\vect{M}_{FL}$子矩阵第$i$行的所有数据均为有效数据，此时停止更新。
	
	最后，使用式~\eqref{eq. distanceMeanTarget}，对$\vect{M}_{(r)}^i$中对应于$\vect{M}_{FL(r)}^i$子矩阵第$i$行的所有数据求取均值，即 $\hat{d}_{i\bar{L}} = \frac{1}{m}\sum \vect{M}_{FL(r)}^i (i,:)$。
	
	\textbf{Return} $\hat{d}_{i\bar{L}}$
	\AlgoBiCaption{第$i$个节点对平均距离$d_{i\bar{L}}$的自组织估计器}{Estimates in agent $i$ for the mean distance $d_{i\bar{L}}$ from followers and leaders}
	\label{alg. SelfInferMeanDistance}
\end{algorithm}

假设~\ref{Ass. SwarmStrenthenTopology}的第一项子内容与假设~\ref{Ass. SwarmTopology}相同，这一项假设的存在是为了确保跟随者集群内部的信息交互是完备的，即保证在有限的通信频次下，每一个跟随者均可以通过通信手段获取其他所有节点的测量数据。假设~\ref{Ass. SwarmStrenthenTopology}的第二项子内容相比于假设~\ref{Ass. SwarmTopology}进行了增强，即从假设~\ref{Ass. SwarmTopology}中领导者仅在初始条件下存在与跟随者之间的通信测量，转变为~\ref{Ass. SwarmStrenthenTopology}中领导者集群需要被跟随者集群时刻、全部地观测到。

定义增广观测矩阵$\vect{M} = \{ d_{ij} | \forall i \in \vect{V}_F, \forall j \in \mathcal{V},\}$，其矩阵维度为$\vect{M} \in \mathbb{R}^{n \times (n+m)}$，$n$是跟随者集群数目，$m$是领导者集群数目。该增广观测矩阵可分解为两个块矩阵形式$\vect{M} = [ \vect{M}_{FF} \quad \vect{M}_{FL} ]$其中$\vect{M}_{FF} \in \mathbb{R}^{n\times n}$ 是跟随者集群内部观测信息子矩阵，$\vect{M}_{FL} \in  \mathbb{R}^{n\times m}$ 是跟随者集群相对于领导者的观测信息子矩阵。

\begin{figure}[htbp]
	\centering		
	\includegraphics[width = 0.97\textwidth]{figures_SURR/distance_estimation_example.png}
	
	\bicaption[fig. distanceEstimationExample]{}{自组织估计器用于估计$d_{i\bar{L}}$案例}{Fig.$\!$}{A representative case for estimating the mean distance $d_{i\bar{L}}$ }
	\vspace{-0.0em}
\end{figure}

给定两个同维度的矩阵$\vect{B}^1, \vect{B}^2 \in \mathbb{R}^{n_1 \times n_2}$，定义一种矩阵之间特殊操作符$\oplus$
为$\vect{B}^3 = \vect{B}^1 \oplus \vect{B}^2$，其中$b^3_{ij}$为$\vect{B}^3$矩阵中处于第$i$行第$j$列的元素，该元素的确定方法为：
$$b^3_{ij} = \left\lbrace \begin{aligned}
	b^1_{ij} + b^2_{ij}  & \quad \text{如果} b^1_{ij} \text{和} b^2_{ij} \text{中至少有一个为零} \\
	b^1_{ij} & \quad \text{否则} \\
\end{aligned} \right.
$$

\begin{figure}[htbp]
	\centering		
	\includegraphics[width = 0.97\textwidth]{figures_SURR/distance_estimation_illustration.png}
	
	\bicaption[fig. distanceEstimationInferProcess]{}{自组织估计器在给定案例下的估计过程，包括自推演和通信交互}{Fig.$\!$}{The evlolution of internal variable in the estimator, including the self-deduction and communication processes}
	\vspace{-0.0em}
\end{figure}

基于以上数据定义，算法~\ref{alg. SelfInferMeanDistance}设计了一类自组织估计方法。对于任意的跟随者节点$i$，该算法仅利用局部测量数据和通信过程中邻接节点分享数据，估计$d_{i\bar{L}}$。该算法包含两个阶段，分别为第2行中的自推演过程，和第4行中的通信融合过程。

为了进一步说明自组织估计算器在运行过程中的细节，图~\ref{fig. distanceEstimationExample}给出了三个领导者和三个跟随者组成的集群案例，以及集群在初始时刻的通信测量关系；和图~\ref{fig. distanceEstimationInferProcess}展示了在一次通信过程中，三个跟随者节点上用于估计增广观测矩阵的估计变量的演变过程。由图~\ref{fig. distanceEstimationInferProcess}可知，初始条件下，每个节点仅依靠自身对邻接节点的观测数据初始化其局部的估计观测矩阵。该观测矩阵本质上是节点使用已知信息对全局拓扑的模拟。首先，每个节点会对模拟的拓扑中同时存在两个以上观测节点（对应于估计观测矩阵中同时存在两个以上有效数据的行），根据式~\eqref{eq. distanceTwoPoint}进行自推演过程，产生新的“观测”数据。该数据在实际的观测中可能并不存在，该数据仅是当前节点在假设该观测存在时，对观测数值的合理推测和估计。然后每个节点通过通信交互与其邻接的跟随者节点共享了其对增广观测矩阵的估计矩阵，将其在自推演阶段产生的新信息传播到了邻接节点。经过异或融合过程，各自节点上生成的新的观测信息将被其邻接节点掌握，并作为已知信息参与邻接节点下一次的自推演更新过程。如此的自推演-通信交互-自推演的循环经过多次重复后，所有节点均可以成功估计出其相距每一个领导者的距离“观测数据”。

定理\ref{th2-5 maximumCommTimesForStability}给出了算法~\ref{alg. SelfInferMeanDistance}中通信融合次数$r$的上限。

\begin{theorem}\label{th2-5 maximumCommTimesForStability}
	如果假设~\ref{Ass. SwarmStrenthenTopology}成立，则对于具有$m$个领导者和$n$个跟随者的多智能体集群，则跟随者集群内部最多执行$n-1$次算法~\ref{alg. SelfInferMeanDistance}后，即$r \le n-1$，所有跟随者节点均可得到准确且稳定的$\hat{d}_{i\bar{L}},\forall i \in \vect{V}_F$。	
\end{theorem}

\begin{proof}
	算法~\ref{Def. SurroundingProblem}的最终目标是推测出所有跟随者节点$ \forall i \in \vect{V}_F$与所有领导者的距离$\hat{d}_{iL_j},\forall L_j \in \vect{V}_F$。因此算法中的重复次数$r$取决于最后一个跟随者完成稳定估计所需的最大通信次数。
	
	基于假设~\ref{Ass. SwarmStrenthenTopology}，跟随者集群内部的通信和观测拓扑是连通的，因此任意两个节点之间都存在一条观测和通信的通路。假设~\ref{Ass. SwarmStrenthenTopology}同时保证了所有的领导者都至少被一个跟随者节点观测到。因此对于网络中的任意一个跟随者节点$\forall i_1 \in \vect{V}_F$，和任意一个领导者节点$\forall L \in \vect{V}_L$，一定存在一个跟随者节点$i_p$，与指定的领导者节点$L_j$存在直接测量$d_{i_p L}$，并且在跟随者集群内部存在一条连通的通信观测路径，该路径的终点$i_1$。记该路径经过的跟随者节点序列为$\mathcal{R} = [i_p,i_{p-1},\cdots,i_2,i_1]$，其路径深度为$p-1$。则相对距离信息在多智能体系统内部的流动和融合过程如下：
	
	(1) 初始条件下，即通信次数$r=0$时，所有跟随者节点中仅$i_p$节点存在两个相对距离测量，分别为其相对领导者节点的测量$d_{i_p L}$，以及其相对路径$\mathcal{R}$上邻接节点$i_{p-1}$的测量$d_{i_p i_{p-1}}$。此时，在节点$i_p$上运行的算法\ref{alg. SelfInferMeanDistance}将根据式~\eqref{eq. distanceTwoPoint}推演出一个新的、不存在的“观测”$d_{i_{p-1} L}$。
	
	(2) 执行一次通信交互后，即$r=1$，节点$i_{p-1}$将接收节点$i_p$推演出的新数据$d_{i_{p-1} L}$。由于节点$i_{p-1}$中还存在其与路径$\mathcal{R}$上邻接节点之间的相对测量数据$d_{i_{p-1} i_{p-2}}$，因此节点$i_{p-1}$也将通过自推演步骤生成一个新的“观测”$d_{i_{p-2} L}$。
	
	(3) 在通信次数分别为$r=2,3,\cdots,p-2$时，节点$i_{p-2},i_{p-3},\cdots,i_2$都接收来自路径$\mathcal{R}$上前一个节点分享的推演数据$d_{i_{p-2} L},d_{i_{p-3} L},\cdots,d_{i_2 L}$，进而根据自身对路径上后一节点的测量$d_{i_{p-2} i_{p-3}},d_{i_{p-3} i_{p-4}},\cdots,d_{i_2 i_1}$,通过自推演过程生成$d_{i_{p-3 L}},d_{i_{p-4 L}}, \cdots, d_{i_1 L}$。最后的$d_{i_1 L}$即为$i_1$节点计算${d}_{i_1 \bar{L}}$的所需数据，不过在通信次数为$r=p-2$次时，该数据$i_2$节点上。
	
	(4) 因此仍需要一次通信，此时$r=p-1$，则$d_{i_1 L}$数据通过通信交互过程得以传输至$i_1$节点上。$i_1$节点只需要执行一次通信融合，而不需要再执行自推演过程，即可以完成对领导者$L$相对距离的估计。
	
	由此可推导出，节点$i_1$完成最终估计任务所需的最大通信次数$r$等于连通路径$\mathcal{R}$的深度$p-1$。又根据图论知识可知，在连通图内，任意连通路径的深度都小于$n-1$，因此算法~\ref{alg. SelfInferMeanDistance}使所有跟随者节点获取对领导者相对距离的最大通信次数$r \le n-1$。
	
	至此，定理~\ref{th2-5 maximumCommTimesForStability}得证。
\end{proof}

下面给出在节点相对领导者集群平均位置的平均距离$d_{i\bar{L}}$未知的情况下，跟随者节点实现包围构型的分布式控制律：
\begin{equation} \label{eq. Controller_d_iL_unKnown}
	\begin{aligned}
		u_i = & k_1 \sum_{j \in \vect{N}_i \cap \vect{V}_F} a_{ij} (v_i - v_j) - k_2 v_i\\
		& + \left[ {\hat{d}}_{i\bar{L}} - \xi sgn \left( \sum_{{j \in \vect{N}_i \cap \vect{V}_F}} a_{ij}(0) d_{ij}(0) \right) \right]
	\end{aligned}
\end{equation}
上式中，${\hat{d}}_{i\bar{L}}$由算法~\ref{alg. SelfInferMeanDistance}估计得到。

基于以上分析，定理~\ref{th2-6. mainTheorem_d_iLunknown}总结了在仅使用相对测量信息，且跟随者节点与领导者集群的平均距离未知情况下，包围构型协同控制问题的结论：

\begin{theorem} \label{th2-6. mainTheorem_d_iLunknown}
	由于GNSS受限或拒止而导致领导者集群和跟随者集群的位置状态无法获得的情况下，如果假设~\ref{Ass. surroundingSize}-\ref{Ass. InitalTopology}以及假设~\ref{Ass. SwarmStrenthenTopology}都成立，则对于具有二阶运动学~\eqref{eq. SecondOrderDynamics}的跟随者集群，采用控制律~\eqref{eq. Controller_d_iL_unKnown}，其中所需的${\hat{d}}_{i\bar{L}}$由算法~\ref{alg. SelfInferMeanDistance}估计得到，当控制参数满足$\frac{k_2}{k_1} > \lambda_{max}(\vect{L})$，$\xi \ge \operatorname{max}_{i \in \vect{V}_L} ||x_i - \bar{x}_L||$，且算法~\ref{alg. SelfInferMeanDistance}执行$n-1$以上的次通信交互时，定义~\ref{Def. SurroundingProblem}的包围控制问题得解。
\end{theorem}
\begin{proof}
	定理~\ref{th2-4. d_i_Lknown}中使用的控制律~\eqref{eq. Controller_d_iL_Known}与本定理中应用的控制律~\ref{eq. Controller_d_iL_unKnown}的唯一区别在于，控制律~\ref{eq. Controller_d_iL_unKnown}使用了算法~\ref{alg. SelfInferMeanDistance}输出的估计值$\hat{d}_{i\bar{L}}$代替真实的平均距离${d}_{i\bar{L}}$。 由于定理~\ref{th2-5 maximumCommTimesForStability}保证了算法~\ref{alg. SelfInferMeanDistance}的稳定性，当跟随者集群执行在每次观测后都执行最多$n-1$次通信后，使用算法~\ref{alg. SelfInferMeanDistance}得到的$\hat{d}_{i\bar{L}} = {d}_{i\bar{L}}$。
	
	由此，定理~\ref{th2-6. mainTheorem_d_iLunknown}可直接由定理~\ref{th2-4. d_i_Lknown}的结果推出。
\end{proof}

与定理~\ref{th2-3 higherDimensions}类似，下面的定理将式~\eqref{eq. Controller_d_iL_unKnown}以及算法~\ref{alg. SelfInferMeanDistance}的包围构型控制器拓展到更高维度空间$d>1$。

\begin{theorem} \label{th2-7 higherDimensions}
	高维度空间$d>1$的包围构型控制任务，假设~\ref{Ass. surroundingSize}-\ref{Ass. InitalTopology}以及假设~\ref{Ass. SwarmStrenthenTopology}成立的，对于具备二阶运动学~\eqref{eq. SecondOrderDynamics}的跟随者集群，当它们的初始构型为超矩形时（如二维空间中的矩形，三维空间中的立方体等），如果控制器参数满足定理~\ref{th2-6. mainTheorem_d_iLunknown}中的条件，则基于相对测量数据的控制律~\eqref{eq. Controller_d_iL_unKnown}以及算法~\ref{alg. SelfInferMeanDistance}可以解决高维度空间包围构型控制问题。
\end{theorem}


\BiSubsection{数值仿真结果}{Simulation results}
本节的仿真实验与小节~\ref{subsec. 2-1 simulation}中的仿真实验在集群规模，通信和观测拓扑，以及节点的初始位置和速度等信息等方面采用相同的集群配置。不同的是，在本小节的仿真中，跟随者集群内的节点无法测量自身的绝对位置状态，也无法获得其相对于领导者平局位置的距离信息。跟随者集群中的节点只能使用测量设备与邻接节点进行相对测量，获得它到邻接节点的相对距离数据。本节选用带有测量误差的距离观测器，常见的无线电测距设备的观测误差与相对距离的平方成正比，即对任意两个节点$i$和$j$，它们之间的相对距离测量模型为：
$$\vect{d}_{ij}^m = \vect{d}_{ij}^{true} + k || \vect{d}_{ij}^{true} ||^2 \vect{n} $$
上式中$\vect{d}_{ij}^{true} = \vect{x}_j - \vect{x}_i$，且$\vect{n} \sim \mathcal{N}(\vect{0},\vect{\Sigma})$为信息矩阵$\vect{\Sigma}$已知的零均值高斯白噪声，$k$为噪音扰动系数常值。

\begin{figure}[htbp]
	\centering		
	\subfigure{}\addtocounter{subfigure}{-2}
	\subfigure[Followers' trajectories when $k=0$]{\subfigure[$k=0$时跟随者集群$50s$轨迹]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Trajectories0.eps}}}	
	\null\hfill
	\subfigure{}\addtocounter{subfigure}{-2}
	\subfigure[Followers' trajectories when $k=0.1$]{\subfigure[$k=0.1$时跟随者集群$50s$轨迹]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Trajectories0.1.eps}}}	
	\null \vfill	
	\subfigure{}\addtocounter{subfigure}{-2}
	\subfigure[Followers' trajectories when $k=0.2$]{\subfigure[$k=0.2$时跟随者集群$50s$轨迹]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Trajectories0.2.eps}}}	
	\null\hfill
	\subfigure{}\addtocounter{subfigure}{-2}
	\subfigure[Followers' trajectories when $k=0.4$]{\subfigure[$k=0.4$时跟随者集群$50s$轨迹]{\includegraphics[width = 0.45\textwidth]{figures_SURR/Trajectories0.4.eps}}}		
	\bicaption[fig. SurroundingFormationEvolutionRelative]{}{不同的观测过程噪声参数条件下跟随者集群的运动轨迹}{Fig.$\!$}{Trajectories of the surrounding control process under differnt measurement noises}
	\vspace{-0.0em}
\end{figure}

图~\ref{fig. SurroundingFormationEvolutionRelative}展示了四种不同的$k$值条件下，跟随者集群使用控制器~\eqref{eq. Controller_d_iL_unKnown}以及算法~\ref{alg. SelfInferMeanDistance}完成包围构型的运动路径。与图~\ref{fig. trajecotoryAbsolutePositions}展示的路径进行比较可以得出，当观测模型不存在观测噪声时，仅使用相对距离测量的控制律~\eqref{eq. Controller_d_iL_unKnown}可以达到与已知绝对位置状态的控制器~\eqref{eq. ControllerWithoutXbar}相同的性能表现。

根据图~\ref{fig. SurroundingFormationEvolutionRelative}进行横向对比可知，观测过程噪声的增大会对跟随者集群在稳定状态下的包围构型产生影响。由于相对距离观测误差的存在，跟随者节点对平均距离$d_{i\bar{L}}$的估计也会存在误差。在仿真前期，由于跟随者集群内部相对距离较近，它们之间的相互观测误差较小，此时观测误差对跟随者节点平均距离估计$d_{i\bar{L}}$的影响较小，控制律~\eqref{eq. Controller_d_iL_unKnown}中起到主要推进作用的是构型参数$\xi$所在项。随着时间增长，跟随者节点之间的相对距离逐渐扩大，它们之间的相对观测误差也随之增加。在仿真末期，由于观测误差导致跟随者节点无法稳定地估计$d_{i\bar{L}}$。在估计误差的作用下，跟随者节点会在包围构型的稳态位置附近进行随机移动。随机运动范围的大小取决于观测误差大小。图~\ref{fig. SurroundingFormationEvolutionRelative}显示，相对距离测量的噪声越大，跟随者节点的随机运动范围越大。


\BiSection{小结}{Summary}
 
本章对具有二阶运动学模型的包围队形协同控制问题进行了研究，首先研究在GNSS信号正常场景下，使用绝对位置状态，设计包围构型的协同控制律。以此为基础，当GNSS信号受限或拒止时，进一步设计了基于相对距离测量信息的分布式协同控制律。仿真结果表明，在不存在观测误差前提下，基于相对测量数据的控制器可以达到使用绝对位置状态设计的控制器同样的控制性能；当观测模型存在误差时，跟随者使用有误差的相对观测依然可以形成包围构型，但是观测误差会影响构型的稳态形状。